/*
 * Copyright (C) 2008 Peter Kling
 * 
 * This file is part of 'A Kind of Billiard', a graphical billiard simulation.
 * 
 * 'A Kind of Billiard' is free software: you can redistribute it and/or modify it under the terms of the GNU General
 * Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any
 * later version.
 * 
 * 'A Kind of Billiard' is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the
 * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
 * details.
 * 
 * You should have received a copy of the GNU General Public License along with 'A Kind of Billiard'. If not, see
 * <http://www.gnu.org/licenses/>.
 */

package de.akob.mathematics;

import java.util.ArrayList;
import java.util.Collections;

/**
 * This class collects some global mathematical methods.
 */
public final class Mathematics {
	/**
	 * Returns the sum of the given matrices.
	 * 
	 * @return sum of given matrices
	 * @throws IllegalArgumentException if matrix dimensions do not match
	 */
	public static MathMatrix add(MathMatrix m1, MathMatrix m2) {
		if (m1.n != m2.n || m2.m != m2.m)
			throw new IllegalArgumentException("given matrices must be of same dimensions");
		MathMatrix result = m1.clone();
		result.add(m2);
		return result;
	}
	
	/**
	 * Returns the sum of two vectors.
	 * 
	 * @return sum of given vectors
	 * @throws IllegalArgumentException if vector dimensions do not match
	 */
	public static MathVector add(MathVector v1, MathVector v2) {
		if (v1.dim != v2.dim)
			throw new IllegalArgumentException("given vectors must be of same dimension");
		MathVector result = v1.clone();
		result.add(v2);
		return result;
	}
	
	/**
	 * Returns whether the given vectors are collinear.
	 * 
	 * @param v1 first vector
	 * @param v2 second vector
	 * @return true if the given vectors are collinear, false otherwise
	 */
	public static boolean collinear(MathVector v1, MathVector v2) {
		return (Math.abs(mul(v1, v2) / (v1.norm() * v2.norm())) == 1.);
	}
	
	/**
	 * Converts the angle given in degree to the corresponding angle in radians.
	 * 
	 * @param deg angle in degree
	 * @return corresponding angle in radians
	 */
	public static double convert_deg2rad(double deg) {
		return Math.PI * deg / 180.;
	}
	
	/**
	 * Converts the angle given in radians to the corresponding angle in degree.
	 * 
	 * @param rad angle in radians
	 * @return corresponding angle in degree
	 */
	public static double convert_rad2deg(double rad) {
		return 180. * rad / Math.PI;
	}
	
	/**
	 * Returns the cross product of the given three dimensional vectors.
	 * 
	 * @param v1 first three dimensional vector
	 * @param v2 second three dimensional vector
	 * @return cross product of given vectors
	 * @throws IllegalArgumentException if one of the vectors is not of dimension three
	 */
	public static MathVector crossProduct(MathVector v1, MathVector v2) {
		if (v1.dim != 3 || v2.dim != 3)
			throw new IllegalArgumentException("at least of one of the given vectors is not of dimension three");
		MathVector result = new MathVector(3);
		result.set(1, v1.get(2) * v2.get(3) - v1.get(3) * v2.get(2));
		result.set(2, v1.get(3) * v2.get(1) - v1.get(1) * v2.get(3));
		result.set(3, v1.get(1) * v2.get(2) - v1.get(2) * v2.get(1));
		return result;
	}
	
	/**
	 * Converts angle given in degree into angle given in radians.
	 * 
	 * @param deg angle in degree
	 * @return given angle in radians
	 */
	public static double deg2rad(double deg) {
		return Math.PI * deg / 180.;
	}
	
	/**
	 * Returns the distance between the given vector and plane.
	 * 
	 * @param plane plane
	 * @param vector vector
	 * @return distance between given vector and plane
	 */
	public static double distance(MathPlane3D plane, MathVector vector) {
		return Math.abs(distance_oriented(vector, plane));
	}
	
	/**
	 * Returns the distance between the given vector and plane.
	 * 
	 * @param vector vector
	 * @param plane plane
	 * @return distance between given vector and plane.
	 */
	public static double distance(MathVector vector, MathPlane3D plane) {
		return Math.abs(distance_oriented(vector, plane));
	}
	
	/**
	 * Returns the distance between two points.
	 * 
	 * @param p1 first point
	 * @param p2 second point
	 * @return distance between given points
	 */
	public static double distance(MathVector p1, MathVector p2) {
		return Math.abs(distance_oriented(p1, p2));
	}
	
	/**
	 * Returns the oriented distance between the given point and plane. If the oriented distance is > 0, the given point
	 * lies in the half space containing the origin.
	 * 
	 * @param plane plane
	 * @param vector vector
	 * @return oriented distance between vector and plane
	 */
	public static double distance_oriented(MathPlane3D plane, MathVector vector) {
		return distance_oriented(vector, plane);
	}
	
	/**
	 * Returns the oriented distance between the given point and plane. If the oriented distance is > 0, the given point
	 * lies in the half space containing the origin.
	 * 
	 * @param vector vector
	 * @param plane plane
	 * @return oriented distance between vector and plane
	 */
	public static double distance_oriented(MathVector vector, MathPlane3D plane) {
		return plane.getDistance() - Mathematics.mul(vector, plane.getNormal());
	}
	
	/**
	 * Returns the oriented distance between two points. More exactly, the length of the vector p2-p1 is returned.
	 * 
	 * @param p1 first point
	 * @param p2 second point
	 * @return oriented distance between given points (length p2-p1)
	 */
	public static double distance_oriented(MathVector p1, MathVector p2) {
		return Mathematics.sub(p2, p1).norm();
	}
	
	/**
	 * Returns a rotation matrix for rotations of phi radians around the given axis. The returned rotation matrix can be
	 * used for three dimensional rotations in the euclidean space.
	 * 
	 * @param phi rotation angle in radians
	 * @param axis rotation axis
	 * @return rotation matrix for given angle and axis
	 * @throws IllegalArgumentException if the given axis is the zero vector or has not exactly three components.
	 */
	public static MathMatrix getRotationMatrix3D(double phi, MathVector axis) {
		if (axis.norm() == 0.)
			throw new IllegalArgumentException("given axis must not be the zero vector");
		if (axis.dim != 3)
			throw new IllegalArgumentException("given axis must be a three dimensional vector");
		
		// construct rotation matrix
		MathMatrix rotation = new MathMatrix(3, 3);
		double cosPhi = Math.cos(phi);
		double sinPhi = Math.sin(phi);
		double norm = axis.norm();
		double x = axis.get(1) / norm;
		double y = axis.get(2) / norm;
		double z = axis.get(3) / norm;
		rotation.set(1, 1, cosPhi + x * x * (1 - cosPhi));
		rotation.set(1, 2, x * y * (1 - cosPhi) - z * sinPhi);
		rotation.set(1, 3, x * z * (1 - cosPhi) + y * sinPhi);
		rotation.set(2, 1, y * x * (1 - cosPhi) + z * sinPhi);
		rotation.set(2, 2, cosPhi + y * y * (1 - cosPhi));
		rotation.set(2, 3, y * z * (1 - cosPhi) - x * sinPhi);
		rotation.set(3, 1, z * x * (1 - cosPhi) - y * sinPhi);
		rotation.set(3, 2, z * y * (1 - cosPhi) + x * sinPhi);
		rotation.set(3, 3, cosPhi + z * z * (1 - cosPhi));
		
		return rotation;
	}
	
	/**
	 * Returns a rotation matrix for rotations of <code>phi</code> radians around the given axis. The returned
	 * rotation matrix can be used for three dimensional rotations using homogeneous coordinates. Note that the given
	 * axis can be provided as an three or four dimensional vector; if it is given as a four dimensional vector, the
	 * fourth component will be ignored (it is interpreted as a homogeneous vector).
	 * 
	 * @param phi rotation angle in radians
	 * @param axis rotation axis
	 * @return rotation matrix for given angle and axis
	 * @throws IllegalArgumentException if the given axis is the zero vector or has not three or four components.
	 */
	public static MathMatrix getRotationMatrix3D_hom(double phi, MathVector axis) {
		if (axis.norm() == 0.)
			throw new IllegalArgumentException("Axis for rotation matrix must be a non-zero vector.");
		if (axis.dim != 3 && axis.dim != 4)
			throw new IllegalArgumentException("Axis for rotation matrix must have exactly three or four components.");
		
		// construct rotation matrix
		MathMatrix rotation = new MathMatrix(4, 4);
		double cosPhi = Math.cos(phi);
		double sinPhi = Math.sin(phi);
		double norm = (axis.dim == 3) ? axis.norm() : (new MathVector(axis.get(1), axis.get(2), axis.get(3))).norm();
		double x = axis.get(1) / norm;
		double y = axis.get(2) / norm;
		double z = axis.get(3) / norm;
		rotation.set(1, 1, cosPhi + x * x * (1 - cosPhi));
		rotation.set(1, 2, x * y * (1 - cosPhi) - z * sinPhi);
		rotation.set(1, 3, x * z * (1 - cosPhi) + y * sinPhi);
		rotation.set(1, 4, 0.);
		rotation.set(2, 1, y * x * (1 - cosPhi) + z * sinPhi);
		rotation.set(2, 2, cosPhi + y * y * (1 - cosPhi));
		rotation.set(2, 3, y * z * (1 - cosPhi) - x * sinPhi);
		rotation.set(2, 4, 0.);
		rotation.set(3, 1, z * x * (1 - cosPhi) - y * sinPhi);
		rotation.set(3, 2, z * y * (1 - cosPhi) + x * sinPhi);
		rotation.set(3, 3, cosPhi + z * z * (1 - cosPhi));
		rotation.set(3, 4, 0.);
		rotation.set(4, 1, 0.);
		rotation.set(4, 2, 0.);
		rotation.set(4, 3, 0.);
		rotation.set(4, 4, 1.);
		
		return rotation;
	}
	
	/**
	 * Returns a unit (<code>n</code> x <code>n</code>) matrix.
	 * 
	 * @param n dimension of the square unit matrix
	 * @return (<code>n</code> x <code>n</code>) unit matrix
	 */
	public static MathMatrix getUnitMatrix(int n) {
		MathMatrix matrix = new MathMatrix(n, n);
		for (int i = 1; i <= n; i++)
			matrix.set(i, i, 1.);
		return matrix;
	}
	
	/**
	 * Returns the <code>i</code>-th <code>n</code>-dimensional unit vector.
	 * 
	 * @param n dimension of vector to return
	 * @param i which unit vector to return
	 * @return <code>i</code>-th <code>n</code>-dimensional unit vector
	 */
	public static MathVector getUnitVector(int n, int i) {
		MathVector v = new MathVector(n);
		v.set(i, 1.);
		return v;
	}
	
	/**
	 * Returns a ascending sorted list of ray-parameters that specify all intersection points of the given ray and
	 * cuboid.
	 * 
	 * @param ray ray to intersect with cuboid
	 * @param cuboid cuboid to intersect with ray
	 * @return sorted (ascending) list of ray-parameters that specify all intersection points of the given ray and
	 *         cuboid
	 */
	public static ArrayList<Double> intersect(Cuboid cuboid, Ray ray) {
		return intersect(ray, cuboid);
	}
	
	/**
	 * Returns a list of ray-parameters that specify all intersection points of the given ray and cuboid. Note that for
	 * rays that lie in one of the bordering planes of the cuboid, only the lowest parameter will be added to the list.
	 * 
	 * @param ray ray to intersect with cuboid
	 * @param cuboid cuboid to intersect with ray
	 * @return list of ray-parameters that specify all intersection points of the given ray and cuboid
	 */
	public static ArrayList<Double> intersect(Ray ray, Cuboid cuboid) {
		ArrayList<Double> results = new ArrayList<Double>();
		MathVector origin = ray.getOrigin();
		MathVector direction = ray.getDirection();
		MathVector rayPoint;
		double minX, maxX, minY, maxY, minZ, maxZ;
		double diff, t;
		
		// intersection left
		if (direction.get(1) != 0) {
			minX = cuboid.getMinX();
			diff = minX - origin.get(1);
			t = diff / direction.get(1);
			rayPoint = ray.evaluateAt(t);
			if (cuboid.contains(rayPoint))
				results.add(t);
		}
		
		// intersection top
		if (direction.get(1) != 0) {
			maxX = cuboid.getMaxX();
			diff = maxX - origin.get(1);
			t = diff / direction.get(1);
			rayPoint = ray.evaluateAt(t);
			if (cuboid.contains(rayPoint))
				results.add(t);
		}
		
		// intersection bottom
		if (direction.get(2) != 0) {
			minY = cuboid.getMinY();
			diff = minY - origin.get(2);
			t = diff / direction.get(2);
			rayPoint = ray.evaluateAt(t);
			if (cuboid.contains(rayPoint))
				results.add(t);
		}
		
		// intersection at top
		maxY = cuboid.getMaxY();
		diff = maxY - origin.get(2);
		if (direction.get(2) != 0) {
			t = diff / direction.get(2);
			rayPoint = ray.evaluateAt(t);
			if (cuboid.contains(rayPoint))
				results.add(t);
		}
		
		// intersection at bottom
		minZ = cuboid.getMinZ();
		diff = minZ - origin.get(3);
		if (direction.get(3) != 0) {
			t = diff / direction.get(3);
			rayPoint = ray.evaluateAt(t);
			if (cuboid.contains(rayPoint))
				results.add(t);
		}
		
		// intersection at top
		maxZ = cuboid.getMaxZ();
		diff = maxZ - origin.get(3);
		if (direction.get(3) != 0) {
			t = diff / direction.get(3);
			rayPoint = ray.evaluateAt(t);
			if (cuboid.contains(rayPoint))
				results.add(t);
		}
		
		// sort and return parameter list
		Collections.sort(results);
		return results;
	}
	
	/**
	 * Returns the reflection of the given point along the given plane.
	 * 
	 * @param plane reflection plane
	 * @param point point to reflect
	 * @return reflection of <code>point</code> along <code>plane</code>
	 */
	public static MathVector mirrorPoint(MathPlane3D plane, MathVector point) {
		return mirrorPoint(point, plane);
	}
	
	/**
	 * Returns the reflection of the given point along the given plane.
	 * 
	 * @param point point to reflect
	 * @param plane reflection plane
	 * @return reflection of <code>point</code> along <code>plane</code>
	 */
	public static MathVector mirrorPoint(MathVector point, MathPlane3D plane) {
		return add(point, mul(plane.getNormal(), 2 * distance_oriented(point, plane)));
	}
	
	/**
	 * Returns the reflection of the given vector along the given plane. The reflection of a vector along a plane is
	 * defined by the vector resulting from mirroring the point given by the vector along the translation of the plane
	 * into the origin.
	 * 
	 * @param plane reflection plane
	 * @param vector vector to reflect
	 * @return reflection of <code>vector</code> along <code>plane</code>
	 */
	public static MathVector mirrorVector(MathPlane3D plane, MathVector vector) {
		return mirrorVector(vector, plane);
	}
	
	/**
	 * Returns the reflection of the given vector along the given plane. The reflection of a vector along a plane is
	 * defined by the vector resulting from mirroring the point given by the vector along the translation of the plane
	 * into the origin.
	 * 
	 * @param vector vector to reflect
	 * @param plane reflection plane
	 * @return reflection of <code>vector</code> along <code>plane</code>
	 */
	public static MathVector mirrorVector(MathVector vector, MathPlane3D plane) {
		MathPlane3D translatedPlane = new MathPlane3D(plane.getNormal(), 0.);
		return mirrorPoint(vector, translatedPlane);
	}
	
	/**
	 * Returns the component wise product of the given vector and scalar.
	 * 
	 * @param scalar scalar
	 * @param vector vector
	 * @return component wise product <code>scalar*vector</code>
	 */
	public static MathVector mul(double scalar, MathVector vector) {
		MathVector result = vector.clone();
		result.mul(scalar);
		return result;
	}
	
	/**
	 * Returns the product of the given matrices.
	 * 
	 * @param m1 first matrix
	 * @param m2 second matrix
	 * @return matrix product of given matrices
	 * @throws IllegalArgumentException if m1.m != m2.n (matrices can not be multiplied).
	 */
	public static MathMatrix mul(MathMatrix m1, MathMatrix m2) {
		if (m1.m != m2.n)
			throw new IllegalArgumentException("matrix dimensions do not match for multiplication");
		MathMatrix result = new MathMatrix(m1.n, m2.m);
		for (int i = 1; i <= result.n; i++) {
			for (int j = 1; j <= result.m; j++) {
				double sum = 0.;
				for (int k = 1; k <= m1.m; k++)
					sum += m1.get(i, k) * m2.get(k, j);
				result.set(i, j, sum);
			}
		}
		return result;
	}
	
	/**
	 * Returns the requested matrix - vector product.
	 * 
	 * @param matrix matrix (left operand)
	 * @param vector vector (right operand)
	 * @return matrix vector product of given operands
	 * @throws IllegalArgumentException if vector dimension does not match matrix column dimension
	 */
	public static MathVector mul(MathMatrix matrix, MathVector vector) {
		if (matrix.m != vector.dim)
			throw new IllegalArgumentException("matrix/vector dimensions do not match for multiplication");
		MathVector result = new MathVector(matrix.n);
		for (int i = 1; i <= result.dim; i++) {
			double value = 0.;
			for (int j = 1; j <= vector.dim; j++)
				value += vector.get(j) * matrix.get(i, j);
			result.set(i, value);
		}
		return result;
	}
	
	/**
	 * Returns the component wise product of the given vector and scalar.
	 * 
	 * @param vector vector
	 * @param scalar scalar
	 * @return component wise product <code>scalar*vector</code>
	 */
	public static MathVector mul(MathVector vector, double scalar) {
		return Mathematics.mul(scalar, vector);
	}
	
	/**
	 * Returns the scalar product of two vectors.
	 * 
	 * @throws IllegalArgumentException if vector dimensions do not match.
	 */
	public static double mul(MathVector v1, MathVector v2) {
		if (v1.dim != v2.dim)
			throw new IllegalArgumentException("vector dimensions do not match for multiplication");
		double result = 0;
		for (int i = 1; i <= v1.dim; i++)
			result += v1.get(i) * v2.get(i);
		return result;
	}
	
	/**
	 * Returns the product of the given quaternions.
	 * 
	 * @param q1 first quaternion
	 * @param q2 second quaternion
	 * @return product of the given quaternions.
	 */
	public static Quaternion mul(Quaternion q1, Quaternion q2) {
		Quaternion q = new Quaternion(0., 0., 0., 0.);
		q.set(1, q1.get(1) * q2.get(1) - q1.get(2) * q2.get(2) - q1.get(3) * q2.get(3) - q1.get(4) * q2.get(4));
		q.set(2, q1.get(1) * q2.get(2) + q1.get(2) * q2.get(1) + q1.get(3) * q2.get(4) - q1.get(4) * q2.get(3));
		q.set(3, q1.get(1) * q2.get(3) - q1.get(2) * q2.get(4) + q1.get(3) * q2.get(1) + q1.get(4) * q2.get(2));
		q.set(4, q1.get(1) * q2.get(4) + q1.get(2) * q2.get(3) - q1.get(3) * q2.get(2) + q1.get(4) * q2.get(1));
		return q;
	}
	
	/**
	 * Converts angle given in radians into angle given in degree.
	 * 
	 * @param rad angle in radians
	 * @return given angle in degree
	 */
	public static double rad2deg(double rad) {
		return 180. * rad / Math.PI;
	}
	
	/**
	 * Rotates the given two dimensional vector angle radians in the mathematical positive sense. This method uses
	 * rotation matrices.
	 * 
	 * @param vector vector to be rotated
	 * @param angle angle the vector is rotated by (in radians)
	 * @throws IllegalArgumentException if given vector is not of dimension two
	 */
	public static void rotation2D_matrix(MathVector vector, double angle) {
		if (vector.dim != 2)
			throw new IllegalArgumentException("given vector must be of dimension two");
		double sinAngle = Math.sin(angle);
		double cosAngle = Math.cos(angle);
		double x = vector.get(1);
		double y = vector.get(2);
		vector.set(1, x * cosAngle - y * sinAngle);
		vector.set(2, x * sinAngle + y * cosAngle);
	}
	
	/**
	 * Rotation using quaternions. The given vector is rotated using the given quaternion.
	 * 
	 * @param v vector to be rotated
	 * @param q (unit) quaternion used for the rotation (result for non-unit quaternions not specified)
	 * @return the rotated vector
	 * @throw IllegalArgumentException if given vector is not of dimension three
	 */
	public static MathVector rotation3D(MathVector v, Quaternion q) {
		if (v.dim != 3)
			throw new IllegalArgumentException("given vector must have dimension three");
		Quaternion pureV = new Quaternion(0., v.get(1), v.get(2), v.get(3));
		Quaternion result = mul(mul(q, pureV), q.inv());
		return new MathVector(result.get(2), result.get(3), result.get(4));
	}
	
	/**
	 * Rotates the given three/four dimensional vector angle radians in the mathematical positive sense around the given
	 * axis. Note that it is more efficient to use directly rotation matrices when rotating more than one vector by the
	 * same axis/angle. If the given vector is four dimensional, it is considered to be a three dimensional vector given
	 * in homogeneous coordinates.
	 * 
	 * @param vector vector to be rotated
	 * @param angle angle the vector is rotated by (in radians)
	 * @param axis axis the vector is rotated around
	 * @throws IllegalArgumentException if given vector is not of dimension three or four
	 */
	public static void rotation3D_matrix(MathVector vector, double angle, MathVector axis) {
		if (vector.dim != 3 && vector.dim != 4)
			throw new IllegalArgumentException("given vector must be of dimension three (four if homogenious)");
		if (vector.dim == 3) {
			MathMatrix rotMatrix = getRotationMatrix3D(angle, axis);
			MathVector tmp = mul(rotMatrix, vector);
			vector.set(1, tmp.get(1));
			vector.set(2, tmp.get(2));
			vector.set(3, tmp.get(3));
		} else {
			MathMatrix rotMatrix = getRotationMatrix3D_hom(angle, axis);
			MathVector tmp = mul(rotMatrix, vector);
			vector.set(1, tmp.get(1));
			vector.set(2, tmp.get(2));
			vector.set(3, tmp.get(3));
			// vector.set(4, tmp.get(4)); // not needed, homogeneous coordinate is not changed by rotation
		}
	}
	
	/**
	 * Rotates the given three dimensional vector angle radians in the mathematical positive sense around the given
	 * axis. Note that it is more efficient to use directly quaternions for rotation when rotating more than one vector
	 * by the same axis/angle.
	 * 
	 * @param vector vector to be rotated
	 * @param angle angle the vector is rotated by (in radians)
	 * @param axis axis the vector is rotated around
	 * @throws IllegalArgumentException if given vector is not of dimension three
	 */
	public static MathVector rotation3D_quaternion(MathVector v, double angle, MathVector axis) {
		if (v.dim != 3)
			throw new IllegalArgumentException("given vector must be of dimension three");
		Quaternion q = new Quaternion(angle, axis);
		return rotation3D(v, q);
	}
	
	/**
	 * Spherical linear interpolation for two unit vectors.
	 * 
	 * @param t interpolation parameter
	 * @param v1 origin vector of interpolation (must be of unit length)
	 * @param v2 destination vector of interpolation (must be of unit length)
	 * @return spherical linear interpolation of given vectors for given interpolation parameter
	 * @throws IllegalArgumentException if the dimension of the given vectors does not match
	 */
	public static MathVector slerp(double t, MathVector v1, MathVector v2) {
		if (v1.dim != v2.dim)
			throw new IllegalArgumentException("given vectors must be of same dimensions");
		double dotProduct = Mathematics.mul(v1, v2);
		int sign = dotProduct < 0 ? -1 : 1;
		double omega = Math.acos(sign * dotProduct);
		double factor1, factor2;
		if (omega != 0.) {
			factor1 = Math.sin((1 - t) * omega) / Math.sin(omega);
			factor2 = sign * Math.sin(t * omega) / Math.sin(omega);
		} else {
			factor1 = 1.;
			factor2 = 0.;
		}
		return Mathematics.add(Mathematics.mul(factor1, v1), Mathematics.mul(factor2, v2));
	}
	
	/**
	 * Spherical linear interpolation for two quaternions.
	 * 
	 * @param t interpolation parameter
	 * @param q1 origin quaternion of interpolation (must be a unit quaternion)
	 * @param q2 destination quaternion of interpolation (must be a unit quaternion)
	 * @return spherical linear interpolation of given quaternions for given interpolation parameter
	 */
	public static Quaternion slerp(double t, Quaternion q1, Quaternion q2) {
		MathVector v1 = new MathVector(q1.get(1), q1.get(2), q1.get(3), q1.get(4));
		MathVector v2 = new MathVector(q2.get(1), q2.get(2), q2.get(3), q2.get(4));
		MathVector interpolated = slerp(t, v1, v2);
		return new Quaternion(interpolated.getElements());
	}
	
	/**
	 * Returns the difference of the given matrices (m1-m2).
	 * 
	 * @return difference of given matrices
	 * @throws IllegalArgumentException if matrix dimensions do not match
	 */
	public static MathMatrix sub(MathMatrix m1, MathMatrix m2) {
		if (m1.n != m2.n || m2.m != m2.m)
			throw new IllegalArgumentException("given matrices must be of same dimensions");
		MathMatrix result = m1.clone();
		result.sub(m2);
		return result;
	}
	
	/**
	 * Returns the difference of two vectors (v1-v2).
	 * 
	 * @return difference of given vectors
	 * @throws IllegalArgumentException if vector dimensions do not match
	 */
	public static MathVector sub(MathVector v1, MathVector v2) {
		if (v1.dim != v2.dim)
			throw new IllegalArgumentException("given matrices must be of same dimensions");
		MathVector result = v1.clone();
		result.sub(v2);
		return result;
	}
}
